/*
 * VisionController.cpp
 *
 */

#include "../../include/vision/VisionController.h"

namespace vctrl {

VisionController::VisionController() {
	pthread_mutex_init(&_mutex,0);

    _kin 		= FreenectAdapter::getInstance();

    _bgr 		= new Mat(Size(FRAME_WIDTH, FRAME_HEIGHT), CV_8UC3);
    _depth 		= new Mat(Size(FRAME_WIDTH, FRAME_HEIGHT), CV_16UC1);

	// Rettangolo per il taglio dell'immagine ottenuta dalla Kinect.
	_cut 		= new Rect(CUT_RECT_X, CUT_RECT_Y, PROC_FRAME_WIDTH, PROC_FRAME_HEIGHT);

	// Creazione delle immagini per il taglio.
	_bgrProc 	= new Mat(Size(PROC_FRAME_WIDTH, PROC_FRAME_HEIGHT), CV_8UC3);
	_depthProc 	= new Mat(Size(PROC_FRAME_WIDTH, PROC_FRAME_HEIGHT), CV_16UC1);

	_mask 	= new Mat(Size(PROC_FRAME_WIDTH, PROC_FRAME_HEIGHT), CV_8UC1);

	 _bctx 		= new  ObjectContext<BallStrategy>;
	 _gctx 		= new  ObjectContext<GoalStrategy>;
	 _octx 		= new  ObjectContext<ObstacleStrategy>;

	 _goal 		= NULL;
	 _ball		= NULL;

	_kin->startBGRCapture();
	_kin->startDepthCapture();
}

VisionController::~VisionController() {
	destroyAllWindows();

	delete _bctx;
	delete _gctx;
	delete _octx;

	delete _mask;
	delete _bgrProc;
	delete _depthProc;
	delete _cut;
	delete _bgr;
	delete _depth;

	_kin->stopBGRCapture();
	_kin->startDepthCapture();
	_kin->release();

	_keypoints.clear();
	_pois.clear();

	pthread_mutex_destroy(&_mutex);

	_bctx 		= NULL;
	_gctx 		= NULL;
	_octx 		= NULL;
	_mask 		= NULL;
	_bgrProc 	= NULL;
	_depthProc 	= NULL;
	_cut 		= NULL;
	_bgr 		= NULL;
	_depth 		= NULL;
	_kin 		= NULL;
}

void VisionController::elaborateImage() {
	pthread_mutex_lock(&_mutex);

	if ((_kin->isBGRStarted() && _kin->isDepthStarted())) {

		if(_pois.size() > 0)
			_pois.clear();

		_kin->getBGRFrame(*_bgr);
		_kin->getDepthFrame(*_depth);

		Utils::imageCutter(*_bgr, *_cut, *_bgrProc);
		Utils::imageCutter(*_depth, *_cut, *_depthProc);

		_bctx->doMask(*_bgrProc, *_mask);
		_bctx->calcKeyPoints(*_mask, _keypoints, *_mask);
		_bctx->extractPOIs(*_depthProc, _keypoints, _pois);
		_keypoints.clear();

		_gctx->doMask(*_bgrProc, *_mask);
		_gctx->calcKeyPoints(*_mask, _keypoints, *_mask);
		_gctx->extractPOIs(*_depthProc, _keypoints, _pois);
		_keypoints.clear();

		_octx->doMask(*_bgrProc, *_mask);
		_octx->calcKeyPoints(*_mask, _keypoints, *_mask);
		_octx->extractPOIs(*_depthProc, _keypoints, _pois);
		_keypoints.clear();

		_ball=NULL;
		_goal=NULL;
		bool ballFound=false;
		bool goalFound=false;
		for (unsigned int i=0;i<_pois.size();i++){
			ObjectType label = _pois[i]->getLabel();
			if (!ballFound && label==BALL ){
				_ball= _pois[i];
				ballFound = true;
			}
			else if (!goalFound && label==GOAL ){
				_goal= _pois[i];
				goalFound = true;
			}
			else{
				if(label==OBSTACLE) {
					_obstacles.push_back(_pois[i]);
				}
			}
		}

		Utils::showPOIs(*_bgrProc, _pois);
		cv::imshow("Monitor", *_bgrProc);
		cv::waitKey(5);

	}

	pthread_mutex_unlock(&_mutex);

}
/*
vector<POI*> VisionController::getPOIs() {
	return _pois;
}*/

void VisionController::alza() {
	_kin->alza();
}

void VisionController::abbassa() {
	_kin->abbassa();
}

void VisionController::azzera() {
	_kin->azzera();
}

float VisionController::getGoalDepth() {
	if(_goal!=NULL) {
		return _goal->getDistance();
	}
	return -1.0f;
}

float VisionController::getBallDepth() {
	if(_ball!=NULL) {
		return _ball->getDistance();
	}
	return -1.0f;
}

float VisionController::getGoalAngle() {
	if(_goal!=NULL) {
		return _goal->getAngle();
	}
	return -361.0f;
}

float VisionController::getBallAngle() {
	if(_ball!=NULL) {
		return _ball->getAngle();
	}
	return -361.0f;
}


}
